/*
*
==============================================================================
*  Name        : 
*  Part of     : Omen Game Engine
*  Description : 
*  Version     : 1.0
*
*  Copyright (c) 2013 Lauri Kortevaara.
* 
==============================================================================
*/

#define OMEN_SOURCE 1

/// Omen Headers
#include <OmenTransform.h>

/// STL Headers
#include <vector>
#include <iostream>

using namespace Omen;

Transform::Transform()
{
    m_localPosition = m_position = Omen::Vector3d(0.0f, 0.0f, 0.0f);
    m_localRotation = m_rotation =  Omen::Quaternion::fromMatrix(Omen::Matrix4x4());

    /*
    Omen::Matrix4x4         m_transformation;       /// The transformation matrix having the rotation and translation

    Omen::Quaternion        m_rotation;             /// Quaternion representing the rotation
    Omen::Quaternion        m_localRotation;        /// Quaternion representing the rotation relative to parent transform
        
    Omen::Vector3d          m_position;             /// Position in world space
    Omen::Vector3d          m_localPosition;        /// Position relative to parent transform
        
    Omen::Vector3d          m_eulerAngles;          /// Rotation in Euler angles
    Omen::Vector3d          m_localEulerAngles;     /// Rotation relative to parent transform

    Omen::Vector3d          m_right;                /// Orthogonal base unit vector representing right direction in left-hand-coordinate system
    Omen::Vector3d          m_up;                   /// Orthogonal base unit vector representing up direction in left-hand-coordinate system
    Omen::Vector3d          m_forward;              /// Orthogonal base unit vector representing forward direction in left-hand-coordinate system

    Omen::Vector3d          m_localScale;           /// Scale-vector relative to parent transform
    */
}

Transform::Transform( const Transform& other )
{
    m_localPosition = other.m_localPosition;
    m_position = other.m_position;
    m_localRotation = other.m_localRotation;
    m_rotation =  other.m_rotation;
}

Transform::~Transform() 
{
}

/// <summary>Translate the transform by given vector3d</summary>
void Transform::translate( const Omen::Vector3d& v )
{
    m_localPosition += v;
}

/// <summary>Translate the transform by given vector3d</summary>
void Transform::setPosition( const Omen::Vector3d& v )
{
    m_localPosition = v;
}

/// <summary>Rotate the transform by Euler angles defined by Vector3d(x,y,z). First around z-axis , then x-, and last the y-axis</summary>
void Transform::rotate( const Omen::Vector3d& r )
{
    m_localRotation *=  Omen::Quaternion::fromAxisAndAngle( 0.0, 0.0, 1.0, r.z ) * 
                        Omen::Quaternion::fromAxisAndAngle( 0.0, 1.0, 0.0, r.y ) *
                        Omen::Quaternion::fromAxisAndAngle( 1.0, 0.0, 0.0, r.x );
}

/// <summary>Rotate the transform by Euler angles defined by Vector3d(x,y,z). First around z-axis , then x-, and last the y-axis</summary>
void Transform::setRotation( const Omen::Vector3d& r )
{
    m_localRotation = Quaternion::fromEulerAngles( r.y,r.x,r.z );
}

/// <summary>Rotate transform about axis passing through point in world coordinates by angle in degrees</summary>
void Transform::rotateAround( const Omen::Vector3d& point, const Omen::Vector3d& axis, const Omen::Float& angle )
{
}
        
/// <summary>Rotates the transform so that it's forward is pointing to the target coordinate</summary>
void Transform::lookAt( const Omen::Vector3d& target )
{
}

/// <summary>Returns the transformation matrix of this transform</summary>
Omen::Matrix4x4& Transform::transformation()
{
    m_transformation = Matrix4x4();
    // Forward is pointing to the z infinity
    Vector3d right( 1.0f, 0.0f, 0.0f );
    Vector3d up( 0.0f, 1.0f, 0.0f );
    Vector3d forward( 0.0f, 0.0f, 1.0f );

    up      = up; //m_localRotation.rotatedVector(up);
    forward = ((m_localRotation*Quaternion(0.0f,forward))*m_localRotation.conjugate()).vector();
    right = Vector3d::crossProduct(up,forward); 

    static Vector3d sr = right;
    static Vector3d su = up;
    static Vector3d sf = forward;

    if( right != sr && up != su && forward != sf )
    {
        std::cout << "Right vector: " << right.x << ", " << right.y << ", " << right.z << std::endl;
        std::cout << "up vector: " << up.x << ", " << up.y << ", " << up.z << std::endl;
        std::cout << "forward vector: " << forward.x << ", " << forward.y << ", " << forward.z << std::endl;
    }
    sr = right;
    su = up;
    sf = forward;

    Omen::Vector3d rotations = m_localRotation.toEulerAngles(m_localRotation);
    rotations.x /= Omen::PI_OVER_180;
    rotations.y /= Omen::PI_OVER_180;
    rotations.z /= Omen::PI_OVER_180;

    //Omen::Vector3d::orthoNormalize( forward, up, right );
    m_transformation.setColumn(0,right);
    m_transformation.setColumn(1,up);
    m_transformation.setColumn(2,forward);

    m_transformation.translate( m_localPosition );
    //m_transformation.lookAt( m_localPosition, forward, up );

    return m_transformation;
}


/// EOF